#include "spline.h"
#include "evaluar.h"

void interpolar(vPunto& puntos, vSpline& spline, int n)
{
	/* n puntos, n-1 splines. */ 
	
	vdouble diag (n-2);
	vdouble diag_otra (n-1);
	/* Representa la diagonal de arriba y de abajo, que son iguales. */	
	
	vdouble res (n-2);
	vdouble c (n);
	
	vdouble h (n-1);
	vdouble a (n-1);
		 
	/* Llenar los vectores. */
	forn(i, n-1)
	{
		h[i] = puntos[i+1].x-puntos[i].x;		
	}
	
	forn(i, n-2)
	{
		diag[i] = 2*(h[i]+h[i+1]);
		diag_otra[i] = h[i];
	}
	
	diag_otra[0] = 0;
	diag_otra[n-2] = 0;
		
	forn(i, n-1)
	{
		/* a[i] = f(x_i+1) - f(x_i). */
		a[i] = puntos[i+1].y-puntos[i].y;	
	}	
	
	forn(i, n-2)
	{
		res[i] = 3.0/h[i+1] * a[i+1] - 3.0/h[i] * a[i];		
	}	

	/* Resolver el sistema con la matriz tridiagonal representada 
	 * por los vectores diag, diag_otra. 
	 * Se usa el algoritmo de matríz tridiagonal.*/
	
	vdouble coef_c (n-2);
	vdouble coef_d (n-2);
	
	coef_c[0] = diag_otra[1]/diag[0];
	coef_d[0] = res[0]/diag[0];
	
	fornx(i, 1, n-2)
	{
		coef_c[i] = diag_otra[i+1]/ (diag[i] - coef_c[i-1] * diag_otra[i]);
		coef_d[i] = (res[i] - coef_d[i-1] * diag_otra[i])/ 	(diag[i] - coef_c[i-1]*diag_otra[i]);
	}
	
	vdouble x (n-2);
	
	x[n-3] = coef_d[n-3];
	fornd(i, n-3)
	{
		x[i] = coef_d[i] - coef_c[i] * x[i+1];
	}	
	
	c[0] = 0;
	c[n-1] = 0;
	
	forn(i, n-2)
	{
		c[i+1] = x[i];
	}
	
	/* Obtener el resto de los coeficientes en función de los c_i. */	
	vdouble b (n-1);
	vdouble d (n-1);
	
	forn(i, n-1)
	{
		d[i] = (c[i+1] - c[i]) / (3.0 * h[i]);
		b[i] = a[i]/h[i] - h[i]/3.0 * (2*c[i] + c[i+1]);
	}
	
	/* Crear el spline con los valores ya calculados. */	
	forn(i, n-1)
	{
		spline[i].x_i = puntos[i].x;
		
		spline[i].min = puntos[i].x;
		spline[i].max = puntos[i+1].x;
		
		/* S(xj) = f (xj)*/
		
		spline[i].a = puntos[i].y;
		spline[i].b = b[i];
		spline[i].c = c[i];
		spline[i].d = d[i];
	}
	
	return;
}

/* (x-S_x(t))^2 + (y - S_y(t))^2 */
double distancia(Punto& click, vSpline& spline_x, vSpline& spline_y, double punto, int n)
{
	double dist_x = (click.x - evaluar(punto, spline_x, n));
	double dist_y = (click.y - evaluar(punto, spline_y, n));
		
	return (dist_x * dist_x + dist_y * dist_y);
}

/* (S_x(t)-x)((S_x)'(t)) + (S_y(t)-y)((S_x)'(t)). */
double derivada(Punto& click, vSpline& spline_x, vSpline& spline_y, double punto, int n)
{
	double res = (click.x - evaluar(punto, spline_x, n))*(-evaluar_derivada_primera(punto, spline_x, n));
	res += (click.y - evaluar(punto, spline_y, n))*(-evaluar_derivada_primera(punto, spline_y, n));
	
	return res;
}

/* (-(S_x)'(t))^2 + (x-S_x(t))*(-(S_x)''(t)) + (-(S_y)'(t))^2 + (y-S_y(t))*(-(S_y)''(t)). */
double derivada_seg(Punto& click, vSpline& spline_x, vSpline& spline_y, double punto, int n)
{
	double res = ( ((-evaluar_derivada_primera(punto, spline_x, n)) * (-evaluar_derivada_primera(punto, spline_x, n))) + ((evaluar(punto, spline_x, n) - click.x) * (-evaluar_derivada_segunda(punto, spline_x, n))) + ((-evaluar_derivada_primera(punto, spline_y, n)) * (-evaluar_derivada_primera(punto, spline_y, n))) + ((evaluar(punto, spline_y, n) - click.y) * (-evaluar_derivada_segunda(punto, spline_y, n))) );
	
	return res;
}

double mas_Proximo(long int cant_puntos, vSpline& spline_x, vSpline& spline_y, Punto& click, Punto& proximo, int n)
{
	/* f(t) = (x-S_x(t))^2 + (y - S_y(t))^2
	 * f'(t) = 2(x-S_x(t))(-(S_x)'(t)) + 2(y - S_y(t))(-(S_x)'(t)) 
	 * f'(t) = 0 => (S_x(t)-x)((S_x)'(t)) + (S_y(t)-y)((S_x)'(t)) = 0 */	 
	
	vdouble t_partes (cant_puntos);
	muestreo_Uniforme(t_partes, cant_puntos);
	vdouble mejores_t (cant_puntos);
	
	double t_biseccion = multi_biseccion(cant_puntos, t_partes, mejores_t, spline_x, spline_y, click, n);	
	double t_regula_falsi = multi_regula_falsi(cant_puntos, t_partes, mejores_t, spline_x, spline_y, click, n);
	cout << endl << "t de bisección: " << t_biseccion << " t de regula falsi: " << t_regula_falsi << endl;
	
	double dist_biseccion = fabs(distancia(click, spline_x, spline_y, t_biseccion, n));
	double dist_regula = fabs(distancia(click, spline_x, spline_y, t_regula_falsi, n));
	
	double t;
	/* Veo cual es realmente el mas chico */
	if (dist_biseccion <= dist_regula)
	{
		t = t_biseccion;
	}
	else
	{
		t = t_regula_falsi;
	}
	
	t = newton(t, click, spline_x, spline_y, n);
	
	cout << "t de newton: " << t << endl;
	
	proximo.x = evaluar(t, spline_x, n);
	proximo.y = evaluar(t, spline_y, n);
	
	return t;
}

double biseccion(double a, double b, Punto& click, vSpline& spline_x, vSpline& spline_y, int n)
{
	/* Si la función no cambia de signo es razonable suponer que el cero no está en ese intervalo, 
	 * si el intervalo es lo suficientemente chico. */
	if( (derivada(click, spline_x, spline_y, a, n)) * (derivada(click, spline_x, spline_y, b, n)) > 0)
	{
		if ( fabs(derivada(click, spline_x, spline_y, a, n)) < fabs(derivada(click, spline_x, spline_y, b, n)))
			return a;
		else
			return b;
	}
	
	/* Me fijo en los bordes. */
	if (fabs(derivada(click, spline_x, spline_y, a, n)) < epsilon)
		return a;
		
	if (fabs(derivada(click, spline_x, spline_y, b, n)) < epsilon)
		return b;
	
	double c = (a+b)/2;
	int cant = 0;
	double aux = (derivada(click, spline_x, spline_y, c, n));
	
	while( fabs(aux) > epsilon && cant < MAX )
	{
		if ((derivada(click, spline_x, spline_y, a, n) * aux < 0))
			b = c;
		else
			a = c;
		
		c = (a+b)/2;
		aux = (derivada(click, spline_x, spline_y, c, n));
		cant++;
		
		//~ cout << "Número de iteraciones: " << cant << " a: " << a << " b: " << b << endl;
		//~ cout << "Número de iteraciones: " << cant << " c: " << c << " Valor actual: " << aux << endl;
	}
		
	return c;
}

double multi_biseccion(long int cant_puntos, vdouble& t_partes, vdouble& mejores_t, vSpline& spline_x, vSpline& spline_y, Punto& click, int n)
{ 
	double t = 0;
	
	forn(i, cant_puntos-1) 
	{
		t = biseccion(t_partes[i], t_partes[i+1], click, spline_x, spline_y, n); 
		mejores_t[i] = t;
	}
	
	double dist_min = fabs(distancia(click, spline_x, spline_y, mejores_t[0], n));
	double dist_aux;
	
	/* Veo cual es realmente el mas chico */
	forn(i, cant_puntos-1)
	{
		dist_aux = fabs(distancia(click, spline_x, spline_y, mejores_t[i], n));
		
		if (dist_aux < dist_min)
		{
			dist_min = dist_aux;
			t = mejores_t[i];		
		}
	}
	
	return t;
}

/* Método de Newton */ 
double newton(double t_0, Punto& click, vSpline& spline_x, vSpline& spline_y, int n)
{	
	//  x_n+1 = x_n - (f(x_0) / f'(x_0))
	  
	/* quiero el cer de la funcion derivada de la distancia que es:
	 * f'(t) = 2(x-S_x(t))(-(S_x)'(t)) + 2(y - S_y(t))(-(S_x)'(t)) 
	 * f'(t) = 0 => (S_x(t)-x)((S_x)'(t)) + (S_y(t)-y)((S_x)'(t)) = 0 
	 * entonces la derivada de la derivada respecto de t es:
	 * f''(t) = (-(S_x)'(t))^2 + (x-S_x(t))*(-(S_x)''(t)) + (-(S_y)'(t))^2 + (y-S_y(t))*(-(S_y)''(t)) */
	
	double t_res = t_0;
	int cant = 0;
	double aux = derivada(click, spline_x, spline_y, t_res, n);
	
	while( fabs(aux) > epsilon && cant < MAX )
	{		
		/*calculo f'(t)*/
		double aux_deriv = derivada_seg(click, spline_x, spline_y, t_res, n);
		
		/*calculo el x_n+1*/
		t_res = t_res - ( aux / aux_deriv);
		cant++;
		
		/*calculo f(t) */
		aux = derivada(click, spline_x, spline_y, t_res, n);
	};
	
	return  t_res;      
}

/* Método de Regula Falsi. */
double regula_falsi(double t_0, double t_1, Punto& click, vSpline& spline_x, vSpline& spline_y, int n)
{
	double t_n_1 = t_0;
	double t_n = t_1;
	double t_res = 0;	
	
	double aux1 = derivada(click, spline_x, spline_y, t_n, n);
	double aux2 = derivada(click, spline_x, spline_y, t_n_1, n);
	
	/* Si la función no cambia de signo es razonable suponer que el cero no está en ese intervalo, 
	 * si el intervalo es lo suficientemente chico. */
	 
	if( (aux1 * aux2) > 0)
	{
		if ( fabs(aux1) < fabs(aux2) )
			return t_n;
		else
			return t_n_1;
	}	
	
	/* Me fijo en los bordes. */
	if (fabs(aux1) < epsilon)
		return t_n;
		
	if (fabs(aux2) < epsilon)
		return t_n_1;
	
	double diff = fabs(t_n - t_n_1);
	int cant = 0;
	t_res = t_n - ((aux1*diff) / (aux1-aux2));
	double aux3 = (derivada(click, spline_x, spline_y, t_res, n));
	
	while( fabs(aux3) > epsilon && cant < MAX )
	{
		if(aux3*aux1 < 0)
		{
			t_n = t_res; 
			t_n_1 = t_n;

		}
		else if(aux3 * aux2 < 0)
		{
			t_n = t_res; 
		}
		
		aux1 = (derivada(click, spline_x, spline_y, t_n, n));		
		aux2 = (derivada(click, spline_x, spline_y, t_n_1, n));
		
		diff = fabs(t_n - t_n_1);
		
		/* Calculo x_{n+1}. */
		t_res = t_n - ((aux1*diff) / (aux1-aux2));
		aux3 = (derivada(click, spline_x, spline_y, t_res, n));	
		
		cant++;	
	}
	
	return t_res;		 
}

double multi_regula_falsi(long int cant_puntos, vdouble& t_partes, vdouble& mejores_t, vSpline& spline_x, vSpline& spline_y, Punto& click, int n)
{ 
	double t = 0;
	
	forn(i, cant_puntos-1) 
	{
		t = regula_falsi(t_partes[i], t_partes[i+1], click, spline_x, spline_y, n); 
		mejores_t[i] = t;
	}
	
	double dist_min = fabs(distancia(click, spline_x, spline_y, mejores_t[0], n));
	double dist_aux;
	
	/* Veo cual es realmente el mas chico */
	forn(i, cant_puntos-1)
	{
		dist_aux = fabs(distancia(click, spline_x, spline_y, mejores_t[i], n));
		
		if (dist_aux < dist_min)
		{
			dist_min = dist_aux;
			t = mejores_t[i];		
		}
	}
	
	return t;
}

int ubicacion(Punto& nuevo, vPunto& puntos)
{
	int pos = 0;
	
	while(puntos[pos].x < nuevo.x)
		pos++;
		
	return pos;
}

void agregar_punto(Punto& nuevo, int posicion, vPunto& puntos, vPunto& nuevosPuntos)
{
	/* Paso los puntos a una lista. */
	lPunto puntosAux;
	std::copy (puntos.begin (), puntos.end (), std::back_inserter (puntosAux)); 
	
	/* Inserto en nuevo punto donde corresponde */
	lPunto::iterator it = puntosAux.begin();
	forn(i, posicion)
		it++;
	puntosAux.insert(it, nuevo);
	
	/* Pasar los puntos a un vector nuevamente. */
	std::copy (puntosAux.begin (), puntosAux.end (), std::back_inserter (nuevosPuntos));
	
	return;
}

void graficar(long int cant_puntos, string nombre_fig, string caption, vSpline& spline_x, vSpline spline_y, int n)
{
	vdouble puntos_evaluacion (cant_puntos);
	muestreo_Uniforme(puntos_evaluacion, cant_puntos);
	vPunto puntos_grafico (cant_puntos);
	evaluar(puntos_evaluacion, puntos_grafico, spline_x, spline_y, n, cant_puntos);
	
	ofstream inputGrafico ("input_grafico.m");
	
	stringstream aux_x;
	stringstream aux_y;
	aux_x << "x = [";
	aux_y << "y = [";		
			
	forn(i, cant_puntos)
	{
		aux_x << puntos_grafico[i].x << " ";			
		aux_y << puntos_grafico[i].y << " ";			
	}
	
	aux_x << "];";
	aux_y << "];";
	
	/* Formato matlab. */
	string x, y;
	x = aux_x.str();
	y = aux_y.str();
	
	inputGrafico << "close all" << endl;
	inputGrafico << x << endl;
	inputGrafico << y << endl;
	inputGrafico << "plot(x,y)" << endl;
	inputGrafico << "xlabel('x')" << endl;
	inputGrafico << "ylabel('y')" << endl;
	inputGrafico << "title('" << caption << "')" << endl;
	inputGrafico << "print -deps " << nombre_fig << ".eps" << endl;
	
	//~ system("matlab < input_grafico.m");
	system("octave --quiet < input_grafico.m");
	
	stringstream aux;
	aux << "mv -f " << nombre_fig << ".eps ./graficos";	
	system(aux.str().c_str());
	
	return;
}

void graficar_multi(long int cant_puntos, string nombre_fig, string caption, vSpline& spline_x_1, vSpline spline_y_1, vSpline& spline_x_2, vSpline spline_y_2, vSpline& spline_x_3, vSpline spline_y_3, int n)
{
	vdouble puntos_evaluacion (cant_puntos);
	muestreo_Uniforme(puntos_evaluacion, cant_puntos);
	vPunto puntos_grafico (cant_puntos);	
	ofstream inputGrafico ("input_grafico.m");
	string x, y;
	
	evaluar(puntos_evaluacion, puntos_grafico, spline_x_1, spline_y_1, n, cant_puntos);
	stringstream aux_x_1;
	stringstream aux_y_1;
	aux_x_1 << "x = [";
	aux_y_1 << "y = [";		
			
	forn(i, cant_puntos)
	{
		aux_x_1 << puntos_grafico[i].x << " ";			
		aux_y_1 << puntos_grafico[i].y << " ";			
	}
	
	aux_x_1 << "];";
	aux_y_1 << "];";
		
	x = aux_x_1.str();
	y = aux_y_1.str();
	
	inputGrafico << "close all" << endl;
	inputGrafico << x << endl;
	inputGrafico << y << endl;
	inputGrafico << "plot(x,y,3)" << endl;
	inputGrafico << "xlabel('x')" << endl;
	inputGrafico << "ylabel('y')" << endl;
	inputGrafico << "title('" << caption << "')" << endl;
	inputGrafico << "hold on" << endl;
	
	evaluar(puntos_evaluacion, puntos_grafico, spline_x_2, spline_y_2, n, cant_puntos);
	
	stringstream aux_x_2;
	stringstream aux_y_2;
	aux_x_2 << "x = [";
	aux_y_2 << "y = [";		
			
	forn(i, cant_puntos)
	{
		aux_x_2 << puntos_grafico[i].x << " ";			
		aux_y_2 << puntos_grafico[i].y << " ";			
	}
	
	aux_x_2 << "];";
	aux_y_2 << "];";
	
	x = aux_x_2.str();
	y = aux_y_2.str();
	
	inputGrafico << x << endl;
	inputGrafico << y << endl;
	inputGrafico << "plot(x,y,1)" << endl;
	inputGrafico << "hold on" << endl;
	
	evaluar(puntos_evaluacion, puntos_grafico, spline_x_3, spline_y_3, n, cant_puntos);
	
	stringstream aux_x_3;
	stringstream aux_y_3;
	aux_x_3 << "x = [";
	aux_y_3 << "y = [";		
			
	forn(i, cant_puntos)
	{
		aux_x_3 << puntos_grafico[i].x << " ";			
		aux_y_3 << puntos_grafico[i].y << " ";			
	}
	
	aux_x_3 << "];";
	aux_y_3 << "];";
	
	x = aux_x_3.str();
	y = aux_y_3.str();
	
	inputGrafico << x << endl;
	inputGrafico << y << endl;
	inputGrafico << "plot(x,y,2)" << endl;
	
	inputGrafico << "print -deps " << nombre_fig << ".eps" << endl;
	
	//~ system("matlab < input_grafico.m");
	system("octave --quiet < input_grafico.m");
	
	stringstream aux;
	aux << "mv -f " << nombre_fig << ".eps ./graficos";	
	system(aux.str().c_str());
	
	return;
}
